/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Team997Threading extends SimpleRobot {

    Team997RobotDrive drivetrain;
    private Joystick leftStick;
    private Joystick rightStick;

    public Team997Threading() {
        // 1+2 are left wheels,  3,4 are right wheels
        // 1 is shooter, 4 is turret, 3 is pickup.
        drivetrain = new Team997RobotDrive(1, 2, 3, 4);
        leftStick = new Joystick(1);
        rightStick = new Joystick(2);
        getWatchdog().setEnabled(true);
        getWatchdog().feed();
    }

    public void autonomous() {
        for (int i = 0; i < 4; i++) {
            getWatchdog().feed();
            drivetrain.drive(0.5, 0.0);     //drive 50% fwd 0% turn
            Timer.delay(2.0);               //wait 2 seconds
            drivetrain.drive(0.0, 0.75);    //drive 0% fwd 5% turn
        }
        drivetrain.drive(0.0, 0.0);
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        Team997ButtonThread buttonThread = new Team997ButtonThread(this);
        buttonThread.start();
        System.out.println("Hello");
        while (true && isOperatorControl() && isEnabled()) //loop until change
        {

            getWatchdog().feed();
            drivetrain.tankDrive(getLeftStick(), getRightStick());    //drive with joysticks

            Timer.delay(0.0005);
        }
        try {
            buttonThread.join();
        } catch (InterruptedException ex) {
            System.out.println(ex.getMessage());
        }
    }

    /**
     * @return the leftStick
     */
    public Joystick getLeftStick() {
        return leftStick;
    }

    /**
     * @return the rightStick
     */
    public Joystick getRightStick() {
        return rightStick;
    }
    public void robotMain() {
        super.robotMain();
        System.out.println("Download successfully completed!");
        
    }
}
